#include "AS5045B.h"
#include "mcc.h"
#define PPTRIMDelay 2

void Read_Magnetic_Field()
{
    AS5045.MagINC=MagINCn_GetValue();
    AS5045.MagDec=MagDNCn_GetValue();
    if(AS5045.LIN&&AS5045.MagINC&&AS5045.MagDec)
        /*红色范围(~<25mT/>~135MT) 不推荐使用*/
        AS5045.Magnetic_Field=0x02;
    else if(AS5045.LIN==0&&AS5045.MagINC&&AS5045.MagDec==0)
        /*黄色范围(-25~45mT / -75~135) 精度下降*/
        AS5045.Magnetic_Field=0x01;
    else if(AS5045.LIN==0&&AS5045.MagINC==0&&AS5045.MagDec==0)
        /*绿色范围(-45~75mT) 无间距变化，磁场输入正常*/
        AS5045.Magnetic_Field=0x00;
    else
        AS5045.Magnetic_Field=0x03;  //无法使用
}

void initPPTRIM()
{
  PDIO_SetDigitalOutput();
  PDIO_SetLow();
}

void clkPulses(unsigned char num)
{
  unsigned char i;
  for (i = 0; i < num; i++)
  {
    CLK_SetHigh(); DELAY_microseconds(PPTRIMDelay);
    CLK_SetLow(); DELAY_microseconds(PPTRIMDelay);
  }
}

void setupCondition() {
  SPI_CS_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  PDIO_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  SPI_CS_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  SPI_CS_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetLow();
  DELAY_microseconds(PPTRIMDelay);
}

void exitCondition() {
  PDIO_SetDigitalOutput();
  DELAY_microseconds(PPTRIMDelay);
  SPI_CS_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  SPI_CS_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  PDIO_SetLow();
  DELAY_microseconds(PPTRIMDelay);
}

void operationModeLoad()
{
  PDIO_SetLow();
  DELAY_microseconds(1);
  SPI_CS_SetHigh();
  DELAY_microseconds(1);
  clkPulses(4);
}

void operationModeRead()
{
  PDIO_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  SPI_CS_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  clkPulses(1);
  PDIO_SetDigitalInput();
}

void operationModeWrite()
{
  SPI_CS_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  clkPulses(3);
}

void operationModeProg()
{
  CLK_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  SPI_CS_SetHigh();
  DELAY_microseconds(PPTRIMDelay);
  CLK_SetLow();
  DELAY_microseconds(PPTRIMDelay);
  clkPulses(4);
}

unsigned char reversebits(unsigned char val) // Endian switch
{
  unsigned char i = 0, result = 0;
  while (i < 8)
  {
    result += (val << i) & 0x80;
    if (i < 7) result = result >> 1;
    i++;
  }
  return result;
}

void pptrimLoad(unsigned char num_bits)
{
  setupCondition();
  operationModeLoad();
  clkPulses(num_bits);
  exitCondition();
}

void pptrimRead(unsigned char *buff, unsigned char num_bits)
{
  unsigned char current_byte = 0;
  unsigned char current_bit = 0;
  unsigned char temp = 0;
  if (!num_bits) return;
  current_byte = num_bits >> 3;
  current_bit = num_bits & ~0x07;
  setupCondition();
  operationModeRead();
  clkPulses(1); // position the first bit to read
  //-- read OTP Data --
  temp = 0;
  temp += PDIO_GetValue();
  for (current_bit = num_bits; current_bit; current_bit--)
  {
    if (((current_bit - 1) & 0x07) == 0)
    {
      buff[current_bit >> 3] = temp;
      temp = 0;
    }
    if (current_bit)
    {
      temp <<= 1;
      CLK_SetHigh();
      DELAY_microseconds(2);
      temp += PDIO_GetValue();
      CLK_SetLow();
      DELAY_microseconds(2);
    }
  }
  exitCondition();
}

void pptrimWrite(unsigned char *buff, unsigned char num_bits)
{
  unsigned char *current_byte;
  unsigned char current_bit = 0;
  unsigned char temp = 0;
  current_byte = buff + ((num_bits - 1) >> 3);
  temp = *current_byte;
  if (num_bits % 8)
    temp <<= 8 - (num_bits % 8);
  setupCondition();
  operationModeWrite();
  //-- send OTP Data
  for (current_bit = num_bits; current_bit; current_bit--)
  {
    if (temp & 0x80)
      PDIO_SetHigh();
    else
      PDIO_SetLow();
    DELAY_microseconds(1);
    CLK_SetHigh();
    DELAY_microseconds(2);// delay, tzapp=2us(typ.)
    CLK_SetLow();
    DELAY_microseconds(PPTRIMDelay);
    temp <<= 1;
    if (((current_bit - 1) & 0x07) == 0)
    {
      temp = *(--current_byte);
    }
  }
  PDIO_SetHigh();
  DELAY_microseconds(1);
  clkPulses(1); // data latched
  // END OTP-Write
  exitCondition();
}

/*********读绝对角度值****************/
/*IO模拟SPI*/
uint16_t Sim_Read_AS5045()
{
    uint16_t out=0;
    uint8_t i;
    out=0;        
    SPI_CS_SetHigh(); 
    CLK_SetHigh(); 
    SPI_CS_SetLow();         //数据输出DO从变为高电平并启动读取操作
    DELAY_microseconds(5);
    
    CLK_SetLow();         //数据在CLK第一个下降沿锁存至输出移位寄存器中
    DELAY_microseconds(5);
    for(i=0;i<15;i++)     //循环取15位信息
    {
        out=out<<1;
        CLK_SetHigh();
       //每个CLK上升沿移出一位数据
        DELAY_microseconds(2);
        out|=DO_GetValue();
        DELAY_microseconds(2);
        CLK_SetLow();
        DELAY_microseconds(2);
    }

    DELAY_microseconds(2);
    SPI_CS_SetHigh();
    CLK_SetHigh();
    DELAY_microseconds(1);
    return (out);
}
 
uint16_t Sim_Angle_Dout()
{
    Read_Magnetic_Field();
    AS5045.Dat=Sim_Read_AS5045();
    AS5045.OCF= (AS5045.Dat>>2) & 0x01;
    AS5045.COF= (AS5045.Dat>>1) & 0x01;
    AS5045.LIN= AS5045.Dat& 0x01;
    AS5045.AngleDigtal= (AS5045.Dat >> 3);
    
    return AS5045.AngleDigtal;
}

uint16_t Angle =0;

void AS5045_Dat()
{
    Angle=Sim_Angle_Dout()*0.00879*10;
    AS5045.Ang_L=Angle;
    AS5045.Ang_H=Angle>>8;
}

void WriteZero()
{
    unsigned char ZeroTemp,PPTrimBuffer[7];
    Sim_Angle_Dout();
     if(AS5045.OCF&&AS5045.COF==0&&AS5045.LIN==0)
    {  
        // Step 1: Read the 18 bit SSI value from the AS5145 (AS5140 uses 16 bit)
        AS5045.AngleDigtal= (Sim_Read_AS5045() >> 3);

        // Step 2: Read the 54 PPTRIM OTP bits from the AS5145
        pptrimRead(PPTrimBuffer, 54);

        // Step 3: Write the actual angle to the zero position field of the OTP bits
        ZeroTemp = AS5045.AngleDigtal>>8;
        ZeroTemp = reversebits(ZeroTemp);
        PPTrimBuffer[4] = ZeroTemp;

        ZeroTemp = (uint8_t)AS5045.AngleDigtal;
        ZeroTemp = reversebits(ZeroTemp);
        PPTrimBuffer[5] = ZeroTemp;

        // Step 4: Write back the 54 PPTRIM OTP bits containing the Zero position (which isthe actual angle) to the AS5145
        pptrimWrite(PPTrimBuffer, 54);
        DELAY_milliseconds(10);
         // Step 5: Read the new angle. Must be 0 (+-1, if the mechanics is not very stable)
        AS5045_Dat();
     }

}
